#include "anti_gyro.h"
#include "../autoaim_node.hpp"
#include "../../../tool/include/global.h"
#include "../../../tool/include/logger.h"
#include "../../../tool/include/util_func.h"


using namespace std;
static void limit_ratio(const double r, double &ratio)
{
    if (ratio < r)
    {
        ratio = r;
        if (ratio > 0.99)
        {
            ratio = 0.99;
        }
    }
}

bool save_gyro_data(const InputOfGyro &data)
{
    //  /*--------------------------*/
    string s = vaddr;
    string addr = "../save/gyroin_" + s;
    ofstream fout;
    fout.open(addr, std::ios::app | std::ios::out);
    if (!fout.is_open())
    {
        LOGE("PREDICT FILE could not open!");
        return false;
    }
    fout << data.target.clock << ",";
    fout << data.target.pitch << ",";
    fout << data.target.pitch_v << ",";
    fout << data.target.pose.mcu_time << ",";
    fout << data.target.x << ",";
    fout << data.target.xy_plane_distance << ",";
    fout << data.target.xy_v << ",";
    fout << data.target.xyz_distance << ",";
    fout << data.target.xyz_v << ",";
    fout << data.target.y << ",";
    fout << data.target.yaw << ",";
    fout << data.target.yaw_v << ",";
    fout << data.target.z << ",";
    fout << data.armor_angle << ",";
    fout << data.armor_switched << ",";
    fout << data.bullet_speed << ",";
    fout << data.is_balance_infantry << ",";
    fout << data.isBigArmor << ",";
    fout << data.yaw_speed << endl;
    fout.close();
    return true;
}

void AntiGyro::update(const InputOfGyro &data)
{
    // RCLCPP_ERROR(autoaim_node->get_logger(), "update start");
    // save_gyro_data(data);
    car_armors_data.push(data);
    same_armor_data.push(data);
    
    judge_rotate_dir(data.yaw_speed, gyro_description.dir);
    
    if (data.is_balance_infantry)
    {
        armor_num = 2;
    }
    else
    {
        armor_num = 4;
    }

    // 即需要在目标更新时判断是否存在装甲板切换
    if (data.armor_switched)
    {
        last_armor_switch_clock = data.target.clock;
        // 记录切换装甲板的时间戳
        armor_switch_clock_recorder.push(data.target.clock);
        // 通过判断当前装甲板与上一次的装甲板的位置判断是左armor还是右armor
        InputOfGyro last_input_of_gyror;
        //RCLCPP_ERROR(autoaim_node->get_logger(), "testbefore");
        if (!car_armors_data.back(last_input_of_gyror, 2))
        {
            return;
        }
        //RCLCPP_ERROR(autoaim_node->get_logger(), "testafter");
        if (data.target.yaw > last_input_of_gyror.target.yaw) // 切换到左侧装甲板， 当前敌方机器人往右转动
        {
            right_switch_targets.push(last_input_of_gyror.target);
            left_switch_targets.push(data.target);
            // gyro_description.dir = ArmorDir::RIGHT;
        }
        else
        { // 切换到右侧装甲板, 当前敌方机器人往左转动
            left_switch_targets.push(last_input_of_gyror.target);
            right_switch_targets.push(data.target);
            // gyro_description.dir = ArmorDir::LEFT;
        }
        Target tmp_target;
        left_switch_targets.back(tmp_target, 1);
        autoaim_node->autoaim_recorder.left_switch_yaw = tmp_target.yaw;
        right_switch_targets.back(tmp_target, 1);
        autoaim_node->autoaim_recorder.right_switch_yaw = tmp_target.yaw;
        // 计算陀螺有关参数
        calculate_gyro_info();
        // 清空数据
        clear_if_armor_switched();
    }

    // 成立条件1：计算当前装甲板与上一次切换时的时间差， 从而判断是否需要调整至无陀螺状态
    // 这里需要考虑是否是刚刚初始化的车辆，last_armor_switch_clock=0 必然得是无陀螺状态
    int64_t delta_clock_bwtween_CurrClock_and_LastArmorSwitchClock = 0.0;
    delta_clock_bwtween_CurrClock_and_LastArmorSwitchClock = data.target.clock - last_armor_switch_clock;
    bool condition_1 = (delta_clock_bwtween_CurrClock_and_LastArmorSwitchClock > 1500 && car_armors_data.length > 90);
    if (condition_1) //
    {
        clear_if_car_switched();
        gyro_description.armor_status = ArmorMotionStatus::NO_GYRO;
        autoaim_node->autoaim_recorder.clearmode = 1;
    }
    // gyro_description_mutex.unlock();
}

void AntiGyro::clear_if_car_switched()
{
    // gyro_description_mutex.lock();
    same_armor_data.clear();
    car_armors_data.clear();
    left_switch_targets.clear();
    right_switch_targets.clear();
    gyro_description.reset();
    last_armor_switch_clock = 0;
    // RCLCPP_INFO(autoaim_node->get_logger(),"clear if car switched!!!");
    // gyro_description_mutex.unlock();
}

void AntiGyro::clear_if_armor_switched()
{
    same_armor_data.clear();
}

void AntiGyro::calculate_gyro_info()
{
    // RCLCPP_INFO(autoaim_node->get_logger(), "陀螺数据计算");
    // 只要进行过两次
    if (left_switch_targets.length < 2)
        return;
    Target now_left_target, now_right_target, last_left_target, last_right_target;
    left_switch_targets.back(now_left_target, 1);
    left_switch_targets.back(last_left_target, 2);
    right_switch_targets.back(now_right_target, 1);
    right_switch_targets.back(last_right_target, 2);

    // 车辆移动距离
    double delta_yaw = 0.5 * ((now_right_target.yaw + now_left_target.yaw) - (last_right_target.yaw + last_left_target.yaw));

    if (delta_yaw < -180)
    {
        delta_yaw = delta_yaw + 360;
    }
    else if (delta_yaw > 180)
    {
        delta_yaw = delta_yaw - 360;
    }

    // 重置时间(1/numarmor的周期)
    double delta_time = static_cast<double>(0.5 * ((now_right_target.clock + now_left_target.clock) - (last_right_target.clock + last_left_target.clock)));

    // 计算gyro_description陀螺周期,初次不取平均
    if (std::abs(gyro_description.period) < 0.01)
    {
        // std::cout<<"now_left_target.clock"<<now_left_target.clock<<","<<last_right_target.clock<<std::endl;
        // 装甲板持续时间time_span<delta_time
        if (gyro_description.dir == ArmorDir::LEFT)
        {
            gyro_description.time_span = (now_left_target.clock - last_right_target.clock) / 1000.0;
        }
        else
        {
            gyro_description.time_span = (now_right_target.clock - last_left_target.clock) / 1000.0;
        }

        gyro_description.period = (armor_num * delta_time / 1000.0);
        // std::cout<<"gyro_description.period777"<<gyro_description.time_span<<","<<gyro_description.period<<std::endl;
        // 计算敌方机器人中心相对于我方机器人移动的yaw角速度
        gyro_description.car_yaw_speed = (delta_yaw / delta_time);
        // 计算敌方机器人左右两侧装甲板对应的yaw角度
        gyro_description.yaw_section = std::abs(now_left_target.yaw - now_right_target.yaw);
        if (gyro_description.yaw_section > 180)
        {
            gyro_description.yaw_section = 360 - gyro_description.yaw_section;
        }
        // 计算装甲板平均z轴高度
        gyro_description.average_armor_z_axis = 0.5 * (now_left_target.z + now_right_target.z);
    }
    else
    {
        // std::cout<<"now_left_target.clock"<<now_left_target.clock<<","<<last_right_target.clock<<std::endl;
        if (gyro_description.dir == ArmorDir::LEFT)
        {
            gyro_description.time_span = 0.7 * gyro_description.time_span + 0.3 * (now_left_target.clock - last_right_target.clock) / 1000.0;
        }
        else
        {
            gyro_description.time_span = 0.7 * gyro_description.time_span + 0.3 * (now_right_target.clock - last_left_target.clock) / 1000.0;
        }
        gyro_description.period = 0.7 * gyro_description.period + 0.3 * (armor_num * delta_time / 1000.0); // 计算周期, 单位s
        // std::cout<<"gyro_description.period777"<<gyro_description.time_span<<","<<gyro_description.period<<std::endl;
        if (std::abs(now_left_target.yaw - now_right_target.yaw) > 180)
        {
            // 计算敌方机器人左右两侧装甲板对应的yaw角度
            gyro_description.yaw_section = 0.7 * gyro_description.yaw_section + 0.3 * (360 - std::abs(now_left_target.yaw - now_right_target.yaw));
        }
        else
        {
            // 计算敌方机器人左右两侧装甲板对应的yaw角度
            gyro_description.yaw_section = 0.7 * gyro_description.yaw_section + 0.3 * std::abs(now_left_target.yaw - now_right_target.yaw);
        }
        // 计算敌方机器人中心相对于我方机器人移动的yaw角速度
        gyro_description.car_yaw_speed = 0.7 * gyro_description.car_yaw_speed + 0.3 * (delta_yaw / delta_time);
        // 计算装甲板平均z轴高度
        gyro_description.average_armor_z_axis = 0.7 * gyro_description.average_armor_z_axis + 0.3 * 0.5 * (now_left_target.z + now_right_target.z);
    }
    autoaim_node->autoaim_recorder.period = gyro_description.period;
    // 判断陀螺状态
    judge_gyro_status_when_armor_switched(now_left_target.xy_plane_distance);

    // 计算平均yaw角度, 该角度主要用于击打中间点，在对方移动较快时有滞后效果，暂时不滤波
    gyro_description.average_yaw = 0.5 * (now_left_target.yaw + now_right_target.yaw);
    // 计算当前左右侧装甲高度
    // 首先取上一次的装甲板的高度， 上一次右侧装甲板的高度理论为当前左侧装甲板的高度, 上一次左侧装甲板的高度理论为当前右侧装甲板的高度
    double tmp_left_armor_z_axix = gyro_description.left_armor_z_axix;
    double tmp_right_armor_z_axix = gyro_description.right_armor_z_axix;
    gyro_description.right_armor_z_axix = 0.2 * tmp_left_armor_z_axix + 0.8 * now_right_target.z;
    gyro_description.left_armor_z_axix = 0.2 * tmp_right_armor_z_axix + 0.8 * now_left_target.z;
    if (gyro_description.dir != ArmorDir::UNKNOWN)
    {
        gyro_description.valid = true;
    }
}

/*
    函数：判断陀螺旋转模式
    结果：
    gyro_description.armor_status
    ArmorMotionStatus::NO_GYRO 无陀螺（对方在旋转，但是旋转速度过慢或者过快？（这个好像有点问题，无所谓））
    ArmorMotionStatus::HIGH_SPEED_OR_FAR_GYRO 正常判断到旋转
*/
// 两次切换装甲板才会被调用（不包含search那个切换）
void AntiGyro::judge_gyro_status_when_armor_switched(double distance)
{
    if (left_switch_targets.length > 2) // 根据陀螺周期和距离进行判断
    {
        // if ((gyro_description.period > 4.0) || ( (gyro_description.period < 0.2) && (gyro_description.armor_status != ArmorMotionStatus::NO_GYRO) ))// 周期超过4s或者是,认为是无陀螺状态
        if (gyro_description.period > 4.0) // 周期超过4s或者是,认为是无陀螺状态
        {
            LOGM("no gyro!");
            gyro_description.armor_status = ArmorMotionStatus::NO_GYRO;
        }
        else
        {
            LOGM("high speed or far gyro!");
            gyro_description.armor_status = ArmorMotionStatus::HIGH_SPEED_OR_FAR_GYRO;
        }
    }
}

void AntiGyro::high_or_far_gyro_predict(double v, int64_t now, SendPack &send_pack)
{
    // gyro_description_mutex.lock();
    if (!gyro_description.valid) // 陀螺状态没有计算出来
    {
        // hit_median(v, now, send_pack);
        send_pack.target_found = false;
        // gyro_description_mutex.unlock();
        return;
    }

    /// 选择不同的陀螺模型进行击打
#ifdef GYRO_MODEL_THREE
    hit_accord_gyro_model_new(v, now, send_pack);
#elif defined GYRO_MODEL
    hit_accord_gyro_model(v, now, send_pack);
#elif defined GYRO_FIX_CAR_CENTER
    hit_center(v, now, send_pack);
#elif defined GYRO_FIND_MIDDLE
    hit_median(v, now, send_pack);
#elif defined GYRO_FIX_CAR_CENTER_AUTO_SHOOT
    aim_center_auto_shoot(v, now, send_pack);

#else
    LOGE("UNKNOWN ANTI GYRO MODE!");
#endif
    // gyro_description_mutex.unlock();
}

// 击打中心就不需要判断陀螺是否有效，直接左右平均即可
void AntiGyro::hit_center(double v, int64_t now, SendPack &send_pack)
{
    std::cout << "hit_center" << std::endl;
    // gyro_description_mutex.lock();
    InputOfGyro latest_data;
    if (!car_armors_data.back(latest_data, 1))
    {
        send_pack.target_found = false;
        // gyro_description_mutex.unlock();
        return;
    }

    send_pack.target_found = true;
    send_pack.pred_yaw = gyro_description.average_yaw;
    auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);
    send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
    send_pack.pitch_palstance = 0.0;
    send_pack.yaw_palstance = 0.0;
    send_pack.yaw_resolution = Util::getYawResolution(latest_data.target, true);
    send_pack.pitch_resolution = Util::getPitchResolution(latest_data.target);
    // gyro_description_mutex.unlock();
}

void AntiGyro::aim_center_auto_shoot(double v, int64_t now, SendPack &send_pack)
{
    // gyro_description_mutex.lock();
    InputOfGyro latest_data;
    if (!car_armors_data.back(latest_data, 1))
    {
        send_pack.target_found = false;
        // gyro_description_mutex.unlock();
        return;
    }

    // 陀螺模型预测时间为子弹飞行时间与（当前时刻-陀螺模型最近armor_switch更新时间）的和 , 单位s
    // 由clock_now计算时间差，单位s
    auto delta_clock = static_cast<double>(now - last_armor_switch_clock) / 1000.0;
    // 根据最近的一帧装甲板后验观测数据计算子弹飞行时间
    double flight_time = 0.0;
    if (!Util::computBulletFlightTime(latest_data.target, v, flight_time))
    {
        send_pack.target_found = false;
        LOGW("get flight time error!");
        // gyro_description_mutex.unlock();
        return;
    }
    double predict_time = flight_time + delta_clock + shoot_delay;
    // 根据陀螺模型直接预测子弹打过去的敌方车辆装甲板处于的yaw位置
    // 计算相对于边界装甲板实际的小数部分
    double t_ratio = static_cast<double>(armor_num * predict_time / gyro_description.period) - static_cast<int>(armor_num * predict_time / gyro_description.period);
    double half_armor_width_ratio = Util::getYawResolution(latest_data.target, latest_data.isBigArmor) / gyro_description.yaw_section;
    if (std::abs(t_ratio - 0.5) < half_armor_width_ratio)
    {
        send_pack.fire = true;
        send_pack.burstShoot = true;
    }
    else
    {
        send_pack.fire = false;
        send_pack.burstShoot = false;
    }
    send_pack.target_found = true;
    send_pack.pred_yaw = gyro_description.average_yaw;
    auto pred_pitch_ = Util::getPitchForPTZUnderGravity(latest_data.target.xy_plane_distance, gyro_description.average_armor_z_axis, v);
    std::cout << "aim_center" << std::endl;

    send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
    send_pack.pitch_palstance = 0.0;
    send_pack.yaw_palstance = 0.0;
    send_pack.yaw_resolution = Util::getYawResolution(latest_data.target, true);
    send_pack.pitch_resolution = Util::getPitchResolution(latest_data.target);

    // 计算是否发弹

    // gyro_description_mutex.unlock();
}

void AntiGyro::hit_median(double v, int64_t now, SendPack &send_pack)
{
    // gyro_description_mutex.lock();

    InputOfGyro latest_data;
    if (!car_armors_data.back(latest_data, 1))
    {
        send_pack.target_found = false;
        // gyro_description_mutex.unlock();
        return;
    }

    // 寻找中间点
    int cnt = static_cast<int>(0.5 * car_armors_data.length) + 1;
    InputOfGyro median_data;
    car_armors_data.back(median_data, cnt);

    std::cout << "hit_median" << std::endl;

    send_pack.target_found = true;
    send_pack.pred_yaw = median_data.target.yaw;
    auto pred_pitch_ = Util::getPitchForPTZUnderGravity(median_data.target, v);
    send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
    send_pack.pitch_palstance = 0.0;
    send_pack.yaw_palstance = 0.0;
    send_pack.yaw_resolution = Util::getYawResolution(median_data.target, true);
    send_pack.pitch_resolution = Util::getPitchResolution(median_data.target);
    // gyro_description_mutex.unlock();
}

void AntiGyro::hit_accord_gyro_model(double v, int64_t now, SendPack &send_pack)
{
    // gyro_description_mutex.lock();
    // 最近更新的一次装甲板
    InputOfGyro latest_data;
    car_armors_data.back(latest_data, 1);

    send_pack.target_found = true;
    send_pack.yaw_resolution = Util::getYawResolution(latest_data.target, true);
    send_pack.pitch_resolution = Util::getPitchResolution(latest_data.target);

    // 计算截取比例r: r = 0.5 * (w车) / (w车 + glibal_yaw_speed)
    double observe_yaw_speed = 4 * gyro_description.yaw_section / gyro_description.period;
    double r = observe_yaw_speed / (observe_yaw_speed + gimbal_max_yaw_speed);

    // 陀螺模型预测时间为子弹飞行时间与（当前时刻-陀螺模型最近armor_switch更新时间）的和 , 单位s
    // 由clock_now计算时间差，单位s
    auto delta_clock = static_cast<double>(now - last_armor_switch_clock) / 1000.0;
    // 根据最近的一帧装甲板后验观测数据计算子弹飞行时间
    double flight_time = 0.0;
    if (!Util::computBulletFlightTime(get_average_xy_distancce(), get_average_z_distancce(), v, flight_time))
    {
        send_pack.target_found = false;
        LOGW("get flight time error!");
        // gyro_description_mutex.unlock();
        return;
    }
    double predict_time = flight_time + delta_clock;
    // 根据陀螺模型直接预测子弹打过去的敌方车辆装甲板处于的yaw位置，计算相对于边界装甲板实际的小数部分即可
    double t_ratio = static_cast<double>(armor_num * predict_time / gyro_description.period) - static_cast<int>(armor_num * predict_time / gyro_description.period);
    limit_ratio(r, t_ratio);
    double delta_yaw = gyro_description.yaw_section * t_ratio;
    // 计算由于机器人底盘运动造成的yaw影响
    // 对于计算出来的机器人相对于我们的yaw进行速度衰减，即: yaw_speed(t) = yaw_speed(0) * e^(-delta_t/const_t)， 其中 const_t 为 1/4 个陀螺周期

    // double now_car_yaw_speed = gyro_description.car_yaw_speed * exp(-1 * (now - last_armor_switch_clock) * armor_num  / (1000 * gyro_description.period));
    double now_car_yaw_speed = gyro_description.car_yaw_speed;

    double car_move_delta_yaw = now_car_yaw_speed * predict_time;
    // 计算转几个整数的1/4小陀螺周期（整数部分）， 从而便于判断瞄准的装甲板z轴高度，若是往右转，则奇数选右侧装甲板，偶数选择左侧装甲板；若是往左转，则偶数选右侧装甲板，奇数选择左侧装甲板
    int quarter_cnts = static_cast<int>(armor_num * predict_time / gyro_description.period);
    bool is_even = false; // 是否是偶数
    if ((quarter_cnts % 2) == 0)
    {
        is_even = true;
    }
    else
    {
        is_even = false;
    }
    // 计算yaw, 注意yaw左边大，右边小
    if (gyro_description.dir == ArmorDir::LEFT) // 往左转
    {
        Target latest_right_target;
        right_switch_targets.back(latest_right_target, 1);
        send_pack.pred_yaw = latest_right_target.yaw + delta_yaw + car_move_delta_yaw;
        send_pack.yaw_palstance = 1 * armor_num * gyro_description.yaw_section / gyro_description.period;

        if (is_even)
        { // 往左转，若是偶数，选择右侧装甲板
            auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.right_armor_z_axix, v);
            send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
        }
        else
        {
            auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.left_armor_z_axix, v);
            send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
        }
    }
    else
    {
        Target latest_left_target;
        left_switch_targets.back(latest_left_target, 1);
        send_pack.pred_yaw = latest_left_target.yaw - delta_yaw + car_move_delta_yaw;
        send_pack.yaw_palstance = -1 * armor_num * gyro_description.yaw_section / gyro_description.period;
        if (is_even)
        {
            std::cout << "is_even" << std::endl;
            auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.left_armor_z_axix, v);
            send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
        }
        else
        {
            auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.right_armor_z_axix, v);
            send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
        }
    }

    // 考虑云台在回枪时理论上击打不中目标，因此选择自动发弹，考虑发弹延时
    double predict_time_with_shoot_delay = flight_time + delta_clock + shoot_delay;
    double t_ratio_with_shoot_delay = static_cast<double>(armor_num * predict_time_with_shoot_delay / gyro_description.period) - static_cast<int>(armor_num * predict_time_with_shoot_delay / gyro_description.period);
    double half_armor_width_ratio = 0.0;
    if (gyro_description.period < 1.0) // 高速小陀螺，选择开启在装甲板刚接触到击打点就开火的策略
    {
        half_armor_width_ratio = Util::getYawResolution(latest_data.target, latest_data.isBigArmor) / gyro_description.yaw_section;
    }
    r = r - half_armor_width_ratio;
    if (r < 0)
    {
        r = 0;
    }
    if (std::abs(t_ratio_with_shoot_delay) > r)
    {
        send_pack.fire = true;
    }
    else
    {
        send_pack.fire = false;
    }
}

// void AntiGyro::hit_accord_gyro_model_new(double v, int64_t now, SendPack &send_pack)
// {
//     // gyro_description_mutex.lock();

//     // 最近更新的一次装甲板
//     InputOfGyro latest_data;
//     car_armors_data.back(latest_data, 1);

//     send_pack.target_found = true;
//     //yaw pitch精度
//     send_pack.yaw_resolution = Util::getYawResolution(latest_data.target, true);
//     send_pack.pitch_resolution = Util::getPitchResolution(latest_data.target);

//     // 计算截取比例r: r = 0.5 * (w车) / (w车 + glibal_yaw_speed)
//     double observe_yaw_speed = gyro_description.yaw_section / gyro_description.time_span;
//     double r = observe_yaw_speed / (observe_yaw_speed + gimbal_max_yaw_speed);

//     // 陀螺模型预测时间为子弹飞行时间与（当前时刻-陀螺模型最近armor_switch更新时间）的和 , 单位s
//     // 由clock_now计算时间差，单位s
//     // auto delta_clock = static_cast<double>(now - last_armor_switch_clock) / 1000.0;
//     Target latest_right_target, latest_left_target;
//     left_switch_targets.back(latest_left_target, 1);
//     right_switch_targets.back(latest_right_target, 1);
//     Target last_right_target,last_left_target;
//     left_switch_targets.back(last_left_target, 2);
//     right_switch_targets.back(last_right_target, 2);

//     //四分之一周期实际对应的yaw数值范围
//     double yaw_section_virtual = gyro_description.yaw_section * gyro_description.period / (armor_num * gyro_description.time_span);

//     double delta_clock;
//     if (gyro_description.dir == ArmorDir::LEFT)
//     {
//         double yawDel=std::fabs(latest_data.target.yaw - latest_right_target.yaw);
//         if(yawDel>180){
//             yawDel=360-yawDel;
//         }
//         delta_clock =
//             (now - latest_data.target.clock) / 1000.0
//             + gyro_description.time_span * (yawDel + (yaw_section_virtual - gyro_description.yaw_section) / 2.0) / gyro_description.yaw_section;
//     }
//     else
//     {
//         double yawDel=std::fabs(latest_data.target.yaw - latest_left_target.yaw);
//         if(yawDel>180){
//             yawDel=360-yawDel;
//         }
//         delta_clock =
//             (now - latest_data.target.clock) / 1000.0
//             + gyro_description.time_span * ( yawDel+ (yaw_section_virtual - gyro_description.yaw_section) / 2.0) / gyro_description.yaw_section;
//     }

//     // 根据最近的一帧装甲板后验观测数据计算子弹飞行时间
//     double flight_time = 0.0;
//     if (!Util::computBulletFlightTime(get_average_xy_distancce(), get_average_z_distancce(), v, flight_time))
//     {
//         send_pack.target_found = false;
//         LOGW("get flight time error!");
//         // gyro_description_mutex.unlock();
//         return;
//     }

//     double predict_time = flight_time + delta_clock;
//     // 根据陀螺模型直接预测子弹打过去的敌方车辆装甲板处于的yaw位置，计算相对于边界装甲板实际的小数部分即可
//     double t_ratio = static_cast<double>(armor_num * predict_time / gyro_description.period) - static_cast<int>(armor_num * predict_time / gyro_description.period);

//     //其实感觉这个问题早就应该处理，这个实际上是不合适的
//     // limit_ratio(r, t_ratio);

//     //预测yaw数值对于切换装甲板切换位置的相对数值
//     double delta_yaw = yaw_section_virtual * t_ratio - (yaw_section_virtual - gyro_description.yaw_section) / 2.0;
//     // 计算由于机器人底盘运动造成的yaw影响
//     // 对于计算出来的机器人相对于我们的yaw进行速度衰减，即: yaw_speed(t) = yaw_speed(0) * e^(-delta_t/const_t)， 其中 const_t 为 1/4 个陀螺周期

//     // double now_car_yaw_speed = gyro_description.car_yaw_speed * exp(-1 * (now - last_armor_switch_clock) * armor_num  / (1000 * gyro_description.period));
//     double now_car_yaw_speed = gyro_description.car_yaw_speed;

//     double car_move_delta_yaw = now_car_yaw_speed * predict_time;
//     // 计算转几个整数的1/4小陀螺周期（整数部分）， 从而便于判断瞄准的装甲板z轴高度，若是往右转，则奇数选右侧装甲板，偶数选择左侧装甲板；若是往左转，则偶数选右侧装甲板，奇数选择左侧装甲板
//     int quarter_cnts = static_cast<int>(armor_num * predict_time / gyro_description.period);

//     bool is_even = false;// 是否是偶数
//     if ((quarter_cnts % 2) == 0) { is_even = true; }
//     else { is_even = false; }

//     bool bool_send_fire = true;
//     // double enddl=5.0;
//     // 计算yaw, 注意yaw左边大，右边小
//     if (gyro_description.dir == ArmorDir::LEFT)// 往左转
//     {
//         send_pack.pred_yaw = latest_right_target.yaw + delta_yaw + car_move_delta_yaw;
//         if ((t_ratio < (1 - gyro_description.time_span * armor_num / gyro_description.period) / 2.0 || t_ratio > 1 - (1 - gyro_description.time_span * armor_num / gyro_description.period) / 2.0)|| send_pack.pred_yaw>last_left_target.yaw)
//         {
//             send_pack.pred_yaw = latest_right_target.yaw;
//             bool_send_fire = false;
//             //仅仅第一次的时候记录一下
//             if (gyro_description.fire_zero == false)
//             {
//                 gyro_description.fire_zero = true;
//                 gyro_description.last_zero_time = now / 1000.0 + flight_time;
//             }
//         }
//         else { gyro_description.fire_zero = false; }
//         //非切换装甲板的情况下保证是单向进行的
//         if (gyro_description.pred_yaw_old > send_pack.pred_yaw && std::abs(gyro_description.pred_yaw_old - send_pack.pred_yaw) < gyro_description.yaw_section / 4
//             && std::abs(gyro_description.pred_yaw_old) > 0.01)
//         {
//             // std::cout << "异常处理-单向旋转" << std::endl;
//             send_pack.pred_yaw = gyro_description.pred_yaw_old;
//         }
//         send_pack.yaw_palstance = 1 * armor_num * yaw_section_virtual / gyro_description.period;

//         if (is_even)
//         {// 往左转，若是偶数，选择右侧装甲板
//             // auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.right_armor_z_axix, v);
//             auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);
//             send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch);// 如果计算失败则使用旧值
//         }
//         else
//         {
//             auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);
//             send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch);// 如果计算失败则使用旧值
//         }
//     }
//     else
//     {
//         send_pack.pred_yaw = latest_left_target.yaw - delta_yaw + car_move_delta_yaw;
//         // send_pack.pred_yaw = latest_left_target.yaw - delta_yaw ;
//         // {
//         //     double CSVOUT[1][10];
//         //     CSVOUT[0][0]=frameTime;
//         //     CSVOUT[0][1]=car_move_delta_yaw;
//         //     doubleCSVWriter(CSVOUT,1,2,"car_move_delta_yaw.csv");
//         // }

//         //虚拟区域
//         if ((t_ratio < (1 - gyro_description.time_span * armor_num / gyro_description.period) / 2.0
//             || t_ratio > 1 - (1 - gyro_description.time_span * armor_num / gyro_description.period) / 2.0)|| send_pack.pred_yaw<last_right_target.yaw)
//         {
//             send_pack.pred_yaw = latest_left_target.yaw;
//             bool_send_fire = false;
//             //仅仅第一次的时候记录一下
//             if (gyro_description.fire_zero == false)
//             {
//                 gyro_description.fire_zero = true;
//                 gyro_description.last_zero_time = now / 1000.0 + flight_time;
//             }
//         }
//         else { gyro_description.fire_zero = false; }
//         //非切换装甲板的情况下保证是单向进行的
//         if (gyro_description.pred_yaw_old < send_pack.pred_yaw && std::abs(gyro_description.pred_yaw_old - send_pack.pred_yaw) < gyro_description.yaw_section / 4&&std::abs(gyro_description.pred_yaw_old) > 0.01)
//         {
//             // std::cout << "异常处理-单向旋转" << std::endl;
//             send_pack.pred_yaw = gyro_description.pred_yaw_old;
//         }
//         send_pack.yaw_palstance = -1 * armor_num * yaw_section_virtual / gyro_description.period;
//         if (is_even)
//         {
//             // auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.left_armor_z_axix, v);
//             auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);

//             send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch);// 如果计算失败则使用旧值
//         }
//         else
//         {
//             // auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.right_armor_z_axix, v);
//             auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);
//             send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch);// 如果计算失败则使用旧值
//         }
//     }

//     //输出上一次的yaw数值和这一次的
//     // std::cout << "pred_yaw_old:" << gyro_description.pred_yaw_old << " send_pack.pred_yaw:" << send_pack.pred_yaw << std::endl;
//     gyro_description.pred_yaw_old = send_pack.pred_yaw;

//     //回枪处理
//     double time_return_gun = gyro_description.yaw_section / (observe_yaw_speed + gimbal_max_yaw_speed);
//     if ((now / 1000.0 + flight_time - gyro_description.last_zero_time) < time_return_gun) { bool_send_fire = false; }

//     if (bool_send_fire == false) { send_pack.fire = false; }
//     else { send_pack.fire =true; }
//     //老回枪处理
//     /*----------------------------*/
//     // // 考虑云台在回枪时理论上击打不中目标，因此选择自动发弹，考虑发弹延时
//     // double predict_time_with_shoot_delay = flight_time + delta_clock + shoot_delay;
//     // double t_ratio_with_shoot_delay =
//     //     static_cast<double>(armor_num * predict_time_with_shoot_delay / gyro_description.period) - static_cast<int>(armor_num * predict_time_with_shoot_delay / gyro_description.period);
//     // double half_armor_width_ratio = 0.0;
//     // if (gyro_description.period < 1.0)// 高速小陀螺，选择开启在装甲板刚接触到击打点就开火的策略
//     // {
//     //     half_armor_width_ratio = Util::getYawResolution(latest_data.target, latest_data.isBigArmor) / gyro_description.yaw_section;
//     // }
//     // r = r - half_armor_width_ratio;
//     // if (r < 0) { r = 0; }
//     // if (std::abs(t_ratio_with_shoot_delay) > r)
//     // {
//     //     send_pack.fire = true;
//     //     send_pack.burstShoot=true;
//     //     if (bool_send_fire == false) { send_pack.fire = false; }
//     // }
//     // else
//     // {
//     //     // send_pack.pred_yaw=send_pack.pred_yaw;
//     //     send_pack.fire = false;
//     // }
//     /*----------------------------*/

//     //输出pred_yaw文件
//     /*----------------------------*/
//     // std::ofstream fout;
//     // std::string save_addr = "../save/csv/pred_yaw.csv";
//     // fout.open(save_addr, std::ios::app | std::ios::out);
//     // if (!fout.is_open()) { std::cout << "unvalid save_addr" << std::endl; }
//     // fout << latest_data.target.clock << ",";
//     // fout << latest_data.target.yaw << ",";//==target.yaw
//     // // fout << int(anti_gyror.flightTime) << ",";
//     // fout << now + flight_time * 1000 << ",";
//     // fout << send_pack.pred_yaw << ",";
//     // fout << send_pack.target_found << ",";
//     // fout << send_pack.fire << ",";
//     // fout << send_pack.burstShoot << std::endl;
//     // fout.close();
//     /*----------------------------*/
// }
void AntiGyro::hit_accord_gyro_model_new(double v, int64_t now, SendPack &send_pack)
{
    autoaim_node->autoaim_recorder.antigyro_test = 0;
    // gyro_description_mutex.lock();

    // std::cout << "burst2:" << send_pack.burstShoot << std::endl;
    // 最近更新的一次装甲板
    InputOfGyro latest_data;
    car_armors_data.back(latest_data, 1);

    send_pack.target_found = true;
    // yaw pitch精度
    send_pack.yaw_resolution = Util::getYawResolution(latest_data.target, true);
    send_pack.pitch_resolution = Util::getPitchResolution(latest_data.target);

    // 计算截取比例r: r = 0.5 * (w车) / (w车 + glibal_yaw_speed)
    double observe_yaw_speed = gyro_description.yaw_section / gyro_description.time_span;
    double r = observe_yaw_speed / (observe_yaw_speed + gimbal_max_yaw_speed);

    // 陀螺模型预测时间为子弹飞行时间与（当前时刻-陀螺模型最近armor_switch更新时间）的和 , 单位s
    // 由clock_now计算时间差，单位s
    // auto delta_clock = static_cast<double>(now - last_armor_switch_clock) / 1000.0;
    Target latest_right_target, latest_left_target;
    left_switch_targets.back(latest_left_target, 1);
    right_switch_targets.back(latest_right_target, 1);
    Target last_right_target, last_left_target;
    left_switch_targets.back(last_left_target, 2);
    right_switch_targets.back(last_right_target, 2);

    // 四分之一周期实际对应的yaw数值范围
    double yaw_section_virtual = gyro_description.yaw_section * gyro_description.period / (armor_num * gyro_description.time_span);

    double delta_clock;
    if (gyro_description.dir == ArmorDir::LEFT)
    {
        double yawDel = std::fabs(latest_data.target.yaw - latest_right_target.yaw);
        if (yawDel > 180)
        {
            yawDel = 360 - yawDel;
        }
        delta_clock =
            (now - latest_right_target.clock) / 1000.0 + gyro_description.time_span * ((yaw_section_virtual - gyro_description.yaw_section) / 2.0) / gyro_description.yaw_section;
    }
    else
    {
        double yawDel = std::fabs(latest_data.target.yaw - latest_left_target.yaw);
        if (yawDel > 180)
        {
            yawDel = 360 - yawDel;
        }
        delta_clock =
            (now - latest_left_target.clock) / 1000.0 + gyro_description.time_span * ((yaw_section_virtual - gyro_description.yaw_section) / 2.0) / gyro_description.yaw_section;
    }

    // 根据最近的一帧装甲板后验观测数据计算子弹飞行时间
    double flight_time = 0.0;
    if (!Util::computBulletFlightTime(get_average_xy_distancce(), get_average_z_distancce(), v, flight_time))
    {
        send_pack.target_found = false;
        LOGW("get flight time error!");
        // gyro_description_mutex.unlock();
        return;
    }

    cv::FileStorage file;
    file.open(g_tune_param_path, cv::FileStorage::READ);

    file["shoot_delay"] >> shoot_delay;

    file.release();//热调参



    double predict_time = flight_time + delta_clock + shoot_delay;
    // 根据陀螺模型直接预测子弹打过去的敌方车辆装甲板处于的yaw位置，计算相对于边界装甲板实际的小数部分即可
    double t_ratio = static_cast<double>(armor_num * predict_time / gyro_description.period) - static_cast<int>(armor_num * predict_time / gyro_description.period);
    double t_ratio2 = static_cast<double>(armor_num * (delta_clock - (now - latest_data.target.clock) / 1000.0) / gyro_description.period) - static_cast<int>(armor_num * (delta_clock - (now - latest_data.target.clock) / 1000.0) / gyro_description.period);
    double del_t_ratio = -t_ratio2 + t_ratio;

    autoaim_node->autoaim_recorder.t_ratio2 = t_ratio2;
    autoaim_node->autoaim_recorder.t_ratio = t_ratio;
    autoaim_node->autoaim_recorder.del_t_ratio = del_t_ratio;
    autoaim_node->autoaim_recorder.predict_time = predict_time;
    /**/
    // std::ofstream fout2;
    // std::string save_addr2 = "../save/csv/sendca.csv";
    // fout2.open(save_addr2, std::ios::app | std::ios::out);
    // if (!fout2.is_open()) { std::cout << "unvalid save_addr" << std::endl; }
    // // fout2<<delta_clock<<","<<t_ratio2<<","<<t_ratio<<std::endl;
    // // fout2<<del_t_ratio<<","<<t_ratio2<<","<<t_ratio<<std::endl;
    // // fout2<<(now - latest_data.target.clock)<<","<<t_ratio2<<","<<t_ratio<<std::endl;
    // // fout2<<(now - latest_right_target.clock)<<","<<yaw_section_virtual<<","<<gyro_description.yaw_section<<std::endl;
    // // fout2<<delta_clock<<","<<(now - latest_left_target.clock)<<","<<gyro_description.time_span * (  (yaw_section_virtual - gyro_description.yaw_section) / 2.0) / gyro_description.yaw_section<<std::endl;

    // fout2.close();
    /*--------------------------*/

    // 其实感觉这个问题早就应该处理，这个实际上是不合适的
    //  limit_ratio(r, t_ratio);

    // 预测yaw数值对于切换装甲板切换位置的相对数值
    //  double delta_yaw = yaw_section_virtual * t_ratio - (yaw_section_virtual - gyro_description.yaw_section) / 2.0;
    double delta_yaw = yaw_section_virtual * del_t_ratio;
    // 计算由于机器人底盘运动造成的yaw影响
    // 对于计算出来的机器人相对于我们的yaw进行速度衰减，即: yaw_speed(t) = yaw_speed(0) * e^(-delta_t/const_t)， 其中 const_t 为 1/4 个陀螺周期

    // double now_car_yaw_speed = gyro_description.car_yaw_speed * exp(-1 * (now - last_armor_switch_clock) * armor_num  / (1000 * gyro_description.period));
    double now_car_yaw_speed = gyro_description.car_yaw_speed;

    double car_move_delta_yaw = now_car_yaw_speed * (flight_time + ((now - latest_data.target.clock) / 1000.0));
    // 计算转几个整数的1/4小陀螺周期（整数部分）， 从而便于判断瞄准的装甲板z轴高度，若是往右转，则奇数选右侧装甲板，偶数选择左侧装甲板；若是往左转，则偶数选右侧装甲板，奇数选择左侧装甲板
    int quarter_cnts = static_cast<int>(armor_num * predict_time / gyro_description.period);

    bool is_even = false; // 是否是偶数
    if ((quarter_cnts % 2) == 0)
    {
        is_even = true;
    }
    else
    {
        is_even = false;
    }

    bool bool_send_fire = true;
    // double enddl=5.0;
    // 计算yaw, 注意yaw左边大，右边小
    if (gyro_description.dir == ArmorDir::LEFT) // 往左转
    {
        send_pack.pred_yaw = latest_data.target.yaw + delta_yaw + car_move_delta_yaw;

//        if ((t_ratio < (1 - gyro_description.time_span * armor_num / gyro_description.period) / 2.0 || t_ratio > 1 - (1 - gyro_description.time_span * armor_num / gyro_description.period) / 2.0) || send_pack.pred_yaw > last_left_target.yaw)
        // if (t_ratio < 0.3 || t_ratio > 1 || send_pack.pred_yaw > last_left_target.yaw)
        if (t_ratio < 0.3 || t_ratio > 0.6)

        {
            // send_pack.pred_yaw = latest_right_target.yaw;
            send_pack.pred_yaw = send_pack.pred_yaw;
            bool_send_fire = false;
            // 仅仅第一次的时候记录一下
            if (gyro_description.fire_zero == false)
            {
                gyro_description.fire_zero = true;
                gyro_description.last_zero_time = now / 1000.0;
            }
            gyro_description.pred_yaw_old = send_pack.pred_yaw;
            autoaim_node->autoaim_recorder.antigyro_test = 1;

        }
        else if( send_pack.pred_yaw > last_left_target.yaw){
            send_pack.pred_yaw = gyro_description.pred_yaw_old;
            bool_send_fire = false;
            // 仅仅第一次的时候记录一下
            if (gyro_description.fire_zero == false)
            {
                gyro_description.fire_zero = true;
                gyro_description.last_zero_time = now / 1000.0;
            }
            gyro_description.pred_yaw_old = send_pack.pred_yaw;
            autoaim_node->autoaim_recorder.antigyro_test = 2;
        }
        else
        {
            gyro_description.pred_yaw_old = send_pack.pred_yaw;
            gyro_description.fire_zero = false;
            autoaim_node->autoaim_recorder.antigyro_test = 3;
        }
        // 非切换装甲板的情况下保证是单向进行的
        // if (gyro_description.pred_yaw_old > send_pack.pred_yaw && std::abs(gyro_description.pred_yaw_old - send_pack.pred_yaw) < gyro_description.yaw_section / 4 && std::abs(gyro_description.pred_yaw_old) > 0.01)
        // {
        //     // std::cout << "异常处理-单向旋转" << std::endl;
        //     send_pack.pred_yaw = gyro_description.pred_yaw_old;
        // }
        // else if (gyro_description.pred_yaw_old < send_pack.pred_yaw && std::abs(gyro_description.pred_yaw_old - send_pack.pred_yaw) > gyro_description.yaw_section / 2 && std::abs(gyro_description.pred_yaw_old) > 0.01)
        // {
        //     send_pack.pred_yaw = gyro_description.pred_yaw_old;
        // }
        send_pack.yaw_palstance = 1 * armor_num * yaw_section_virtual / gyro_description.period;

        if (is_even)
        { // 往左转，若是偶数，选择右侧装甲板
            auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.right_armor_z_axix, v);
            // auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);
            send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
        }
        else
        {
            auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.left_armor_z_axix, v);
            // auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);
            send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
        }
    }
    else//往右转
    {
        send_pack.pred_yaw = latest_data.target.yaw - delta_yaw + car_move_delta_yaw;
        // send_pack.pred_yaw = latest_left_target.yaw - delta_yaw ;
        // {
        //     double CSVOUT[1][10];
        //     CSVOUT[0][0]=delta_yaw;
        //     CSVOUT[0][1]=car_move_delta_yaw;
        //     doubleCSVWriter(CSVOUT,1,2,"car_move_delta_yaw.csv");
        // }
        /**/
        // std::ofstream fout2;
        // std::string save_addr2 = "../save/csv/pre_old_yaw.csv";
        // fout2.open(save_addr2, std::ios::app | std::ios::out);
        // // if (!fout2.is_open()) { std::cout << "unvalid save_addr" << std::endl; }
        // //  fout2<< send_pack.pred_yaw<<","<<latest_data.target.yaw - delta_yaw<<std::endl;
        // fout2 << latest_data.target.yaw << "," << delta_yaw << std::endl;
        // // fout2<<delta_clock<<","<<t_ratio2<<","<<t_ratio<<std::endl;
        // // fout2<<del_t_ratio<<","<<t_ratio2<<","<<t_ratio<<std::endl;
        // // fout2<<(now - latest_data.target.clock)<<","<<t_ratio2<<","<<t_ratio<<std::endl;
        // // fout2<<(now - latest_right_target.clock)<<","<<yaw_section_virtual<<","<<gyro_description.yaw_section<<std::endl;
        // // fout2<<delta_clock<<","<<(now - latest_left_target.clock)<<","<<gyro_description.time_span * (  (yaw_section_virtual - gyro_description.yaw_section) / 2.0) / gyro_description.yaw_section<<std::endl;

        // fout2.close();
        /*--------------------------*/

        // 虚拟区域
        //if ((t_ratio < (1 - gyro_description.time_span * armor_num / gyro_description.period)  || t_ratio > 1 - (1 - gyro_description.time_span * armor_num / gyro_description.period) ) || send_pack.pred_yaw < last_right_target.yaw)
        // if (t_ratio < 0.3 || t_ratio > 1 || send_pack.pred_yaw < last_left_target.yaw)
        if (t_ratio < 0.3 || t_ratio > 0.6)

        {
            // send_pack.pred_yaw = latest_left_target.yaw;
            send_pack.pred_yaw = send_pack.pred_yaw;
            bool_send_fire = false;
            // 仅仅第一次的时候记录一下
            if (gyro_description.fire_zero == false)
            {
                gyro_description.fire_zero = true;
                gyro_description.last_zero_time = now / 1000.0;
            }
            gyro_description.pred_yaw_old = send_pack.pred_yaw;
            autoaim_node->autoaim_recorder.antigyro_test = 4;
        }
        else if( send_pack.pred_yaw < last_right_target.yaw){
            send_pack.pred_yaw = gyro_description.pred_yaw_old;
            bool_send_fire = false;
            // 仅仅第一次的时候记录一下
            if (gyro_description.fire_zero == false)
            {
                gyro_description.fire_zero = true;
                gyro_description.last_zero_time = now / 1000.0;
            }
            gyro_description.pred_yaw_old = send_pack.pred_yaw;
            autoaim_node->autoaim_recorder.antigyro_test = 5;
        }
        else
        {
            gyro_description.pred_yaw_old = send_pack.pred_yaw;
            gyro_description.fire_zero = false;
            autoaim_node->autoaim_recorder.antigyro_test = 6;
        }
        // 非切换装甲板的情况下保证是单向进行的
        // if (gyro_description.pred_yaw_old < send_pack.pred_yaw && std::abs(gyro_description.pred_yaw_old - send_pack.pred_yaw) < gyro_description.yaw_section / 4 && std::abs(gyro_description.pred_yaw_old) > 0.01)
        // {
        //     // std::cout << "异常处理-单向旋转" << std::endl;
        //     send_pack.pred_yaw = gyro_description.pred_yaw_old;
        // }
        // else if (gyro_description.pred_yaw_old > send_pack.pred_yaw && std::abs(gyro_description.pred_yaw_old - send_pack.pred_yaw) > gyro_description.yaw_section / 2 && std::abs(gyro_description.pred_yaw_old) > 0.01)
        // {
        //     send_pack.pred_yaw = gyro_description.pred_yaw_old;
        // }
        send_pack.yaw_palstance = -1 * armor_num * yaw_section_virtual / gyro_description.period;
        if (is_even)
        {
            auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.left_armor_z_axix, v);
            // auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);

            send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
        }
        else
        {
            auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), gyro_description.right_armor_z_axix, v);
            // auto pred_pitch_ = Util::getPitchForPTZUnderGravity(get_average_xy_distancce(), get_average_z_distancce(), v);
            send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值
        }
    }

    // 输出上一次的yaw数值和这一次的
    //  std::cout << "pred_yaw_old:" << gyro_description.pred_yaw_old << " send_pack.pred_yaw:" << send_pack.pred_yaw << std::endl;

    // 回枪处理
    // double time_return_gun = gyro_description.yaw_section / (observe_yaw_speed + gimbal_max_yaw_speed);
    // if ((now / 1000.0 - gyro_description.last_zero_time) < time_return_gun)
    // {
    //     bool_send_fire = false;
    // }

    if (bool_send_fire == false)
    {
        send_pack.fire = false;
    }
    else
    {
        send_pack.fire = true;
    }
    // 老回枪处理
    /*----------------------------*/
    // // 考虑云台在回枪时理论上击打不中目标，因此选择自动发弹，考虑发弹延时
    // double predict_time_with_shoot_delay = flight_time + delta_clock + shoot_delay;
    // double t_ratio_with_shoot_delay =
    //     static_cast<double>(armor_num * predict_time_with_shoot_delay / gyro_description.period) - static_cast<int>(armor_num * predict_time_with_shoot_delay / gyro_description.period);
    // double half_armor_width_ratio = 0.0;
    // if (gyro_description.period < 1.0)// 高速小陀螺，选择开启在装甲板刚接触到击打点就开火的策略
    // {
    //     half_armor_width_ratio = Util::getYawResolution(latest_data.target, latest_data.isBigArmor) / gyro_description.yaw_section;
    // }
    // r = r - half_armor_width_ratio;
    // if (r < 0) { r = 0; }
    // if (std::abs(t_ratio_with_shoot_delay) > r)
    // {
    //     send_pack.fire = true;
    //     send_pack.burstShoot=true;
    //     if (bool_send_fire == false) { send_pack.fire = false; }
    // }
    // else
    // {
    //     // send_pack.pred_yaw=send_pack.pred_yaw;
    //     send_pack.fire = false;
    // }
    /*----------------------------*/

    // send_pack.burstShoot=true;
    // std::cout << "burst1:" << send_pack.burstShoot << std::endl;
    // // 输出pred_yaw文件
    // /*----------------------------*/
    // std::ofstream fout;
    // std::string save_addr = "../save/csv/anti_pred_yaw.csv";
    // fout.open(save_addr, std::ios::app | std::ios::out);
    // // if (!fout.is_open()) { std::cout << "unvalid save_addr" << std::endl; }
    // fout << latest_data.target.clock << ",";
    // fout << latest_data.target.yaw << ","; //==target.yaw
    // // fout << int(anti_gyror.flightTime) << ",";
    // fout << now + flight_time * 1000 << ",";
    // fout << send_pack.pred_yaw << ",";
    // fout << send_pack.target_found << ",";
    // fout << send_pack.fire << ",";
    // fout << send_pack.burstShoot << std::endl;
    // fout.close();
    /*----------------------------*/
}

bool AntiGyro::If_gyro_description_updated(int64_t now)
{
    autoaim_node->autoaim_recorder.delta_delta_time = now - last_armor_switch_clock;
    if ((now - last_armor_switch_clock) > 1500)
    {
        // RCLCPP_INFO(autoaim_node->get_logger(), "实时性检测不通过");
        return false;
    }
    else
    {
        //RCLCPP_INFO(autoaim_node->get_logger(), "实时性检测通过");
        return true;
    }
}

void AntiGyro::show_result(cv::Mat &canvas, double v, int64_t now)
{
    int width = canvas.cols;
    int height = canvas.rows;
    //
    // 展示当前陀螺状态
    if (this->gyro_description.armor_status == ArmorMotionStatus::NO_GYRO)
    {
        cv::putText(canvas, "no gyro!", cv::Point(420, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    }
    else if (this->gyro_description.armor_status == ArmorMotionStatus::UNKNOWN)
    {
        cv::putText(canvas, "unknown armor mode!", cv::Point(420, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    }
    else
    {
        cv::putText(canvas, "high speed or far gyro!", cv::Point(420, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    }
    // 展示陀螺转向
    if (this->gyro_description.dir == ArmorDir::LEFT)
    {
        cv::putText(canvas, "dir: left", cv::Point(420, 40), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    }
    else if (this->gyro_description.dir == ArmorDir::UNKNOWN)
    {
        cv::putText(canvas, "dir: unknown", cv::Point(420, 40), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    }
    else
    {
        cv::putText(canvas, "dir: right", cv::Point(420, 40), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    }
    // if (this->gyro_description.valid)
    {
        // 陀螺周期
        cv::putText(canvas, "period: " + std::to_string(this->gyro_description.period) + "s", cv::Point(420, 60), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
    }

    if (this->gyro_description.armor_status == ArmorMotionStatus::HIGH_SPEED_OR_FAR_GYRO && left_switch_targets.length > 0)
    {
        Target left_target, right_target;
        left_switch_targets.back(left_target, 1);
        right_switch_targets.back(right_target, 1);
        // 展示左侧和右侧的线
        cv::Point2i left_point, right_point;
        Util::back_projection(left_target, left_point, left_target.pose);
        Util::back_projection(right_target, right_point, right_target.pose);
        cv::line(canvas, cv::Point2i(left_point.x, 0), cv::Point2i(left_point.x, height), cv::Scalar(0, 255, 0), 1);
        cv::line(canvas, cv::Point2i(right_point.x, 0), cv::Point2i(right_point.x, height), cv::Scalar(0, 255, 0), 1);
    }

#ifdef GYRO_FIND_MIDDLE // 寻找历史中位数

#elif defined GYRO_FIX_CAR_CENTER

#elif defined GYRO_MODEL
    // 计算当前帧的预测
    SendPack send_pack_;
    if (this->gyro_description.armor_status == ArmorMotionStatus::HIGH_SPEED_OR_FAR_GYRO)
    {
        this->high_or_far_gyro_predict(v, now, send_pack_);
        InputOfGyro lasted_data;
        if (!car_armors_data.back(lasted_data, 1))
        {
            return;
        }

        Target posterior_target_ = lasted_data.target;
        // 单位度
        posterior_target_.yaw = send_pack_.pred_yaw + 90;
        posterior_target_.x =
            posterior_target_.xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
        posterior_target_.y =
            posterior_target_.xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);
        cv::Point2i predict_point;
        Util::back_projection(posterior_target_, predict_point, lasted_data.target.pose);
        // cv::circle(canvas, predict_point, 10, cv::Scalar(255, 255, 0), -1, cv::LINE_8);
    }
#endif
}

double AntiGyro::get_average_xy_distancce()
{
    double average_xy_distance = 0;
    for (int i = car_armors_data.head; i != car_armors_data.tail; i = ((i + 1) % car_armors_data.size))
    {
        average_xy_distance = average_xy_distance + car_armors_data.values[i].target.xy_plane_distance;
    }
    return (average_xy_distance / car_armors_data.length);
}

double AntiGyro::get_average_z_distancce()
{
    double average_z_distance = 0;
    for (int i = car_armors_data.head; i != car_armors_data.tail; i = ((i + 1) % car_armors_data.size))
    {
        average_z_distance = average_z_distance + car_armors_data.values[i].target.z;
    }
    return (average_z_distance / car_armors_data.length);
}

double AntiGyro::get_average_same_armor_z_distancce()
{
    double same_armor_z_distancce = 0;
    for (int i = same_armor_data.head; i != same_armor_data.tail; i = ((i + 1) % same_armor_data.size))
    {
        same_armor_z_distancce = same_armor_z_distancce + same_armor_data.values[i].target.z;
    }
    return (same_armor_z_distancce / same_armor_data.length);
}

void AntiGyro::judge_rotate_dir(double yaw_speed, ArmorDir &dir)
{
    const int cnt_thresh = 5;
    static int left_dir_cnt = 0;
    if (yaw_speed > 2)
    {
        ++left_dir_cnt;
    }
    else if (yaw_speed < -2)
    {
        --left_dir_cnt;
    }
    if (left_dir_cnt > cnt_thresh)
    {
        dir = ArmorDir::LEFT;
        left_dir_cnt = 5;
    }
    else if (left_dir_cnt < -cnt_thresh)
    {
        dir = ArmorDir::RIGHT;
        left_dir_cnt = -5;
    }
    else
    {
        dir = ArmorDir::UNKNOWN;
    }
}